/*
 * Copyright (c) 2011, The Bishops Arm Project All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.  Redistributions in binary
 * form must reproduce the above copyright notice, this list of conditions and
 * the following disclaimer in the documentation and/or other materials
 * provided with the distribution.  Neither the name of the The Bishops Arm
 * Project nor the names of its contributors may be used to endorse or promote
 * products derived from this software without specific prior written
 * permission.

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

package se.joinenea.bishopsarm;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;

import android.graphics.Color;
import android.util.Log;

public class NXTColorSensorHandler {

    private static final String TAG = "BishopsArm.NXTColorSensorHandler";
    private static final byte COLOR_PORT = (byte) 0x02;
    private final NXTBot nxtBot;

    public NXTColorSensorHandler(NXTBot nxtBot) {
        this.nxtBot = nxtBot;
    }

    public void initColorSensor() throws IOException {
        Log.d(TAG, "Initiating color sensor");
        ByteBuffer set = IOUtils.createBuffer();
        set.put((byte) 0x00);
        set.put((byte) 0x05);
        set.put(COLOR_PORT);
        set.put(SensorType.COLOR_FULL.getValue());
        set.put(SensorMode.RAW_MODE.getValue());

        nxtBot.sendMessage(set);
        nxtBot.readPacket();

        Log.d(TAG, "Color sensor initiated");
    }

    public int getColorVal() throws IOException {
        Log.d(TAG, "Reading color sensor");

        ByteBuffer get = IOUtils.createBuffer();
        get.put((byte) 0x00);
        get.put((byte) 0x07);
        get.put(COLOR_PORT);

        long start = System.currentTimeMillis();
        nxtBot.sendMessage(get);

        byte[] response = nxtBot.readPacket();
        long time = System.currentTimeMillis() - start;
        Log.d(TAG, "got response after=" + time + " ms");
        Log.d(TAG, "get input values response=" + StringUtils.toHex(response));

        // byte 0 : 0x02
        // byte 1 : 0x07
        // byte 2 : Status byte (0x00 if no error)
        // byte 3 : Input port (0x00 to 0x03)
        // byte 4 : Valid ? (Boolean, True(0x01) if new data value should be
        // seen as valid data)
        // byte 5 : Calibrated (Boolean, True(0x01) if calibration file found) i
        // don't test that so in message the byte is at 0x00
        // byte 6 : Sensor type, here 0x0D for color sensor mode
        // byte 7 : Sensor mode, here 0x00 for raw mode
        // byte 8 - 9 : Raw A/D value, here 0x00 0x00
        // byte 10 - 11 : Normalizer A/D Value, here 0x00 0x00
        // byte 12 - 13 Scaled value, here appear the value of the color
        // Black 0x01 0x00
        // Blue 0x02 0x00
        // Green 0x03 0x00
        // Yellow 0x04 0x00
        // Red 0x05 0x00
        // White 0x06 0x00
        // byte 14 - 15 Calibrated value, currently unused so 0x00 0x00

        ByteBuffer msg =
                ByteBuffer.wrap(response).order(ByteOrder.LITTLE_ENDIAN);

        short val = msg.getShort(12);
        Log.d(TAG, "Color sensor value=" + val);

        int result = 0x00;
        switch (val) {
        case 0x01:
            result = Color.BLACK;
            break;
        case 0x02:
            result = Color.BLUE;
            break;
        case 0x03:
            result = Color.GREEN;
            break;
        case 0x04:
            result = Color.YELLOW;
            break;
        case 0x05:
            result = Color.RED;
            break;
        case 0x06:
            result = Color.WHITE;
            break;
        }
        return result;
    }
    private enum SensorType {
        REFLECTION(0x03), LIGHT_ACTIVE(0x05), COLOR_FULL(0x0d);

        private final int value;

        private SensorType(int value) {
            this.value = value;
        }

        public byte getValue() {
            return (byte) value;
        }
    }

    private enum SensorMode {
        RAW_MODE(0x00);

        private final int value;

        private SensorMode(int value) {
            this.value = value;
        }

        public byte getValue() {
            return (byte) value;
        }
    }
}
